#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// 卡尔曼滤波器结构体
typedef struct {
    float x;  // 状态估计值
    float P;  // 估计误差协方差
    float Q;  // 过程噪声方差
    float R;  // 测量噪声方差
    float K;  // 卡尔曼增益
} KalmanFilter;

// 初始化卡尔曼滤波器
void kalmanInit(KalmanFilter *kf, float initial_x, float initial_P, float Q, float R)
{
    kf->x = initial_x;  // 初始估计值
    kf->P = initial_P;  // 初始估计误差协方差
    kf->Q = Q;          // 过程噪声方差
    kf->R = R;          // 测量噪声方差
}

// 卡尔曼滤波更新
float kalmanUpdate(KalmanFilter *kf, float measurement)
{
    // 预测步骤
    // 状态预测：x = x_prev (假设系统模型为x_k = x_{k-1})
    // 估计误差协方差预测：P = P_prev + Q
    kf->P += kf->Q;
    
    // 更新步骤
    // 计算卡尔曼增益：K = P / (P + R)
    kf->K = kf->P / (kf->P + kf->R);
    
    // 更新状态估计：x = x + K*(measurement - x)
    kf->x += kf->K * (measurement - kf->x);
    
    // 更新估计误差协方差：P = (1 - K)*P
    kf->P = (1 - kf->K) * kf->P;
    
    return kf->x;
}

// 示例：使用卡尔曼滤波器处理带噪声的数据
int main()
{
    // 生成一些模拟数据（正弦波 + 噪声）
    const int numSamples = 100;
    float t[numSamples];
    float trueValue[numSamples];
    float measuredValue[numSamples];

    for (int i = 0; i < numSamples; i++) {
        t[i] = i * 0.1f;
        trueValue[i] = 5.0f * sin(t[i]);  // 真实值：正弦波
        
        // 模拟测量值：真实值 + 随机噪声
        // 使用简单的伪随机数生成器
        float noise = ((float)rand() / RAND_MAX) * 2.0f - 1.0f;  // 范围: [-1, 1]
        measuredValue[i] = trueValue[i] + noise;
    }

    // 初始化卡尔曼滤波器
    KalmanFilter kf;
    // 参数说明：初始估计值、初始误差协方差、过程噪声、测量噪声
    kalmanInit(&kf, 0.0f, 1.0f, 0.01f, 0.1f);

    // 应用卡尔曼滤波
    float filteredValue[numSamples];
    for (int i = 0; i < numSamples; i++) {
        filteredValue[i] = kalmanUpdate(&kf, measuredValue[i]);
    }

    // 输出结果（可以导入到Excel或其他工具中绘图比较）
    printf("时间,真实值,测量值,滤波后的值\n");
    for (int i = 0; i < numSamples; i++) {
        printf("%.2f,%.4f,%.4f,%.4f\n", 
               t[i], trueValue[i], measuredValue[i], filteredValue[i]);
    }
    
    return 0;
}
